from vpython import *
import math
from time import sleep
import time

## 初始化各个模块
scene=canvas(width=800,height=200,center=vector(10,0,0),background=color.cyan)
box=box(pos=vector(0,0,0),size=vector(10,10,10))

l=20
arrowlist0 = []
arrowlist1 = []
theta=0
# 上平面
for i in range(-4, 4, 2):
    for j in range(-4, 4, 2):
        arrowlist0.append(arrow(pos=vector(i + l * math.cos(theta), 6 + l * math.sin(theta), j),
                               axis=vector(-l * math.cos(theta), -l * math.sin(theta), 0), shaftwidth=0.1,
                               color=vector(1, 1, 0)))
for i in range(-4, 4, 2):
    for j in range(-4, 4, 2):
        arrowlist1.append(arrow(pos=vector(6 + l * math.cos(theta), i + l * math.sin(theta), j),
                               axis=vector(-l * math.cos(theta), -l * math.sin(theta), 0), shaftwidth=0.1,
                               color=vector(1, 1, 0)))
#设置背景
gd=graph(title="角度变化图",width=600,height=400,xtitle="时间",ytitle="角度")
#创建角度-时间曲线显示窗口
xt=gcurve(graph=gd,color=color.red)

def myplot(theta):
    for i in arrowlist0:
        i.pos.x = + l * math.cos(theta)
        i.pos.y = 6 + l * math.sin(theta)
        i.axis = vector(-l * math.cos(theta), -l * math.sin(theta), 0)

    for j in arrowlist1:
        j.pos.x = 6 + l * math.cos(theta)
        j.pos.y = l * math.sin(theta)
        j.axis = vector(-l * math.cos(theta), -l * math.sin(theta), 0)
import serial
import re
import math

def ReceiveSerial(mySerial):
    while True:
        data=str(serial.read_all())
        if data=="":
            continue
        else:
            break
        sleep(0.02)
    return data

if __name__=="__main__":
    serial = serial.Serial('COM7', 9600, timeout=0.5)
    if (serial.isOpen()):
        print("open success!")
    else:
        print("openfailed!")
    start_time=time.time()
    while True:
        data = ReceiveSerial(serial)
        if data != "b''":
            pattern = re.compile(r"\d+.\d+")
            listdata = pattern.findall(data)
            if len(listdata) == 2:
                print("i= " + listdata[0] + " j= " + listdata[1])


                #规划后的角度角度制
                angle=9 / 4 * math.degrees(math.atan((float(listdata[1])) / float(listdata[0]))) - 54
                print(angle)
                #规划后的角度弧度制
                rad=math.radians(angle)
                print(rad)
                print("***********")
                myplot(rad)
                xt.plot(time.time()-start_time,angle)
            else:
                continue
        else:
            continue